﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using ZedGraph;


namespace FTFSmartCar
{
    
    public partial class IRSensorViewer : Form
    {
        #region visualization variables
        
        PointPairList IRPointPairList = new PointPairList();
        GraphPane MyPane = new GraphPane();
        double[] IRSensorValues = new double[NumIRSensors];
        public const int NumIRSensors = 16;

        public const double SpatialSampling = 0.375;

 
        short[] weights = new short[NumIRSensors] { -45, -39, -33, -27, -21, -15, -9, -3, 3, 9, 15, 21, 27, 33, 39, 45 };
        

        double Centroid;
        short IntCentroid;
        double IntCentroidConverted;

        short TotalMassTest;
        short MomentTest;



        double IRMax, IRMin, IRSignalStrength;
        
        #endregion

        public IRSensorViewer()
        {
            InitializeComponent();
            InitIRViewer();
            ResizeGraph();
        }

        void InitIRViewer()
        {
            MyPane.Title.Text = "IR Sensor View ";
            MyPane.XAxis.Title.Text = "Space (Inches)";
            MyPane.YAxis.Title.Text = "Relative Intensity";
            MyPane.YAxis.Scale.MaxAuto = false;
            MyPane.YAxis.Scale.MinAuto = false;
            MyPane.YAxis.Scale.Min = 0;
            MyPane.YAxis.Scale.Max = 1.0;
            MyPane.YAxis.MajorGrid.IsVisible = true;

            MyPane.XAxis.Scale.MaxAuto = false;
            MyPane.XAxis.Scale.MinAuto = false;
            MyPane.XAxis.Scale.Min = -(SpatialSampling * NumIRSensors/2);
            MyPane.XAxis.Scale.Max = (SpatialSampling * NumIRSensors / 2);
            MyPane.XAxis.MajorGrid.IsVisible = true;

            IRGraph.MasterPane.PaneList.Clear();
            IRGraph.MasterPane.PaneList.Add(MyPane);

            IRGraph.AxisChange();

            for (int i = 0; i < NumIRSensors; i++)
            {
                IRSensorValues[i] = 0.0;
            }
        }
        public void PostIRSensorReadings(byte[] IRSensorsRaw)
        {
            if (IRSensorsRaw.Length == NumIRSensors)
            {
                for (int i = 0; i < NumIRSensors; i++)
                {
                    IRSensorValues[i] = (double)IRSensorsRaw[i] / 255.0;
                }
            }

            CalculateCentroid(IRSensorsRaw);
            CalculateIntCentroid(IRSensorsRaw);

            UpdateGraph();
        }


        public  void CalculateCentroid(byte [] IRSensorsRaw)
        {

            
            double x = 0;
            double Moment = 0;
            double TotalMass = 0;
            byte Min = 255;
            byte Max = 0;

            for (int i = 0; i < NumIRSensors; i++)
            {
                x = (double)(i) * (double)SpatialSampling - (((double)NumIRSensors / (double)2) * (double)SpatialSampling) + ((double)SpatialSampling / 2);

                TotalMass += IRSensorsRaw[i];
                Moment += x * IRSensorsRaw[i];

                if (IRSensorsRaw [i]< Min)
                {
                    Min = IRSensorsRaw[i];
                }   

                if (IRSensorsRaw[i]>Max)
                {
                    Max = IRSensorsRaw[i];
                }

                IRMax = (double)Max/(double)255.0;
                IRMin = (double)Min/(double)255.0;
                IRSignalStrength = IRMax - IRMin;
            }

            Centroid =  (Moment / TotalMass);
        }

        public void CalculateIntCentroid(byte[] IRSensorsRaw)
        {
            short Moment = 0;
            short TotalMass = 0;

            for (int i = 0; i < NumIRSensors; i++)
            {
                TotalMass += (IRSensorsRaw[i]);

                // weights is a constant weighting vector in units of 1/16 inch.  Shifting to the left by 4 bits scales them to the proper size to be represented in Q7.8 without additional operations.
                // the bitshift by 8 is for Q7.8 multiplication
                Moment += (short)(((int)(weights[i] << 4) * (int)(IRSensorsRaw[i])) >> 8);
            }
            
            // Preshift by 8 for Q7.8 division
            IntCentroid = (short)(((int)Moment<<8)/ (int)TotalMass);

            //convert Q7.8 to float for output purposes only
            IntCentroidConverted = (float)IntCentroid / 256.0;         
        }

        private void UpdateGraph()
        {
            double x = 0;

            IRPointPairList.Clear();
            for (int i = 0; i < NumIRSensors; i++)
            {
                x = (double)(i) * (double)SpatialSampling  - (((double)NumIRSensors / (double)2) * (double)SpatialSampling) + ((double)SpatialSampling / 2);
                IRPointPairList.Add(x, IRSensorValues[i]);
            }

            MyPane.GraphObjList.Clear();
            MyPane.CurveList.Clear();

           LineItem IRCurve = MyPane.AddCurve("Relative IR Intensity", IRPointPairList, Color.Blue, SymbolType.Square);

           String IRStats;

           IRStats = "Centroid [" + String.Format("{0:0.00}",Centroid) + "]\r\n";
           IRStats += "Int. Centroid [" + String.Format("{0:0.00}", IntCentroidConverted) + "]\r\n";
           IRStats += "Signal Range [" + String.Format("{0:0.00}",IRMin) + ":"  + String.Format("{0:0.00}",IRMax) + "]\r\n";
           IRStats += "Signal Stregth [" +String.Format("{0:0.00}",IRSignalStrength) + "]";

            TextObj MyText = new TextObj(IRStats ,0,.8);

            MyPane.GraphObjList.Add(MyText);
            IRCurve.Line.IsAntiAlias = true;
           IRCurve.Line.IsSmooth = true;
         
            IRGraph.AxisChange();
            IRGraph.Invalidate();
            this.Invalidate();
        }
        private void IRSensorViewer_FormClosing(object sender, FormClosingEventArgs e)
        {
            e.Cancel = true;
            this.Hide();
        }

        private void IRSensorViewer_Resize(object sender, EventArgs e)
        {
            IRGraph.Location = new Point(10, 10);
            // Leave a small margin around the outside of the control
            IRGraph.Size = new Size(ClientRectangle.Width - 20,
                                    ClientRectangle.Height - 20);

            UpdateGraph();
            this.Invalidate();
        }

        void ResizeGraph()
        {
            IRGraph.Location = new Point(10, 10);
            // Leave a small margin around the outside of the control
            IRGraph.Size = new Size(ClientRectangle.Width - 20,
                                    ClientRectangle.Height - 20);

            UpdateGraph();
            this.Invalidate();
        }
    }
}
